#include "proj.h"

static CanTxMsg CanMsgOutgoingFast;
static CanTxMsg CanMsgOutgoing;

extern int16 Net_CurrPhasU;
extern int16 Net_CurrPhasV;

extern int16 Foc_CurrD;
extern int16 Foc_CurrQ;
extern int16 Foc_CurrAlpha;
extern int16 Foc_CurrBeta;
extern int16 Foc_VoltReqQ;
extern int16 Foc_VoltReqD;
extern uint16 Foc_VoltReqAlpha;
extern uint16 Foc_VoltReqBeta;
extern uint16 Emi_CurQReq;

extern boolean Net_FlgHallSens1;
extern boolean Net_FlgHallSens2;
extern boolean Net_FlgHallSens3;

extern int16 RotSpd_HallSnsrRPM;
extern uint16 FuSpd_PosRot;
extern uint16 RotSpd_PosRotHallSnsr;
extern uint8 HallCalc_IndexPlaus;
extern uint8 RotSpd_St;

#ifndef _RELEASE
extern uint16 os_max_count;
#endif

extern uint32 HallCalc_TickDifAry[7];

void CanOut_initialize()
{
	CAN_OperatingModeRequest(CAN1, CAN_OperatingMode_Normal);
}

void CanOutFast_step()
{
	static boolean loc_bToggel = false;
	static boolean loc_bSkip = false;

	if (loc_bSkip)
	{
		loc_bSkip = false;
		return;
	}

	loc_bSkip = true;

	CanMsgOutgoingFast.DLC = 8;
	CanMsgOutgoingFast.IDE = CAN_ID_STD;
	CanMsgOutgoingFast.RTR = CAN_RTR_DATA;

	/*
	 * Botschaft: FU_STAT1_MEAS (0x170)
	 */

	if (loc_bToggel)
	{

		CanMsgOutgoingFast.StdId = 0x177;
		CanMsgOutgoingFast.Data[0] = RotSpd_HallSnsrRPM & 0xff;
		CanMsgOutgoingFast.Data[1] = (RotSpd_HallSnsrRPM>>8) & 0xff;
		CanMsgOutgoingFast.Data[2] = FuSpd_PosRot & 0xff;
		CanMsgOutgoingFast.Data[3] = (FuSpd_PosRot>>8) & 0xff;
		CanMsgOutgoingFast.Data[4] = (HallCalc_IndexPlaus & 0x7) | ((RotSpd_St & 0x7) << 3);
		CanMsgOutgoingFast.Data[5] = 0;
		CanMsgOutgoingFast.Data[6] = RotSpd_PosRotHallSnsr & 0xff;
		CanMsgOutgoingFast.Data[7] = (RotSpd_PosRotHallSnsr>>8) & 0xff;


	}
	else
	{
		CanMsgOutgoingFast.StdId = 0x176;
		CanMsgOutgoingFast.Data[0] = Foc_CurrD & 0xff;
		CanMsgOutgoingFast.Data[1] = (Foc_CurrD>>8) & 0xff;
		CanMsgOutgoingFast.Data[2] = Foc_CurrQ & 0xff;
		CanMsgOutgoingFast.Data[3] = (Foc_CurrQ>>8) & 0xff;
		CanMsgOutgoingFast.Data[4] = Net_CurrPhasU & 0xff;
		CanMsgOutgoingFast.Data[5] = (Net_CurrPhasU>>8) & 0xff;
		CanMsgOutgoingFast.Data[6] = Foc_VoltReqQ & 0xff;
		CanMsgOutgoingFast.Data[7] = (Foc_VoltReqQ>>8) & 0xff;
	}
	loc_bToggel = !loc_bToggel;

/*


*/
	if (CAN1->TSR & CAN_TSR_TME0 || CAN1->TSR & CAN_TSR_TME1 || CAN1->TSR & CAN_TSR_TME2)
	{
		CAN_Transmit(CAN1, &CanMsgOutgoingFast);
	}

}

void CanOut_step()
{
	CanMsgOutgoing.DLC = 8;
	CanMsgOutgoing.IDE = CAN_ID_STD;
	CanMsgOutgoing.RTR = CAN_RTR_DATA;

	/*
	 * Botschaft: FU_STAT1 (0x70)
	 */
	CanMsgOutgoing.StdId = 0x174;
	CanMsgOutgoing.Data[0] = Emi_CurQReq & 0xff;
	CanMsgOutgoing.Data[1] = (Emi_CurQReq>>8) & 0xff;
	CanMsgOutgoing.Data[2] = 0;
	CanMsgOutgoing.Data[3] = 0;
	CanMsgOutgoing.Data[4] = RotSpd_HallSnsrRPM & 0xff;
	CanMsgOutgoing.Data[5] = (RotSpd_HallSnsrRPM>>8) & 0xff;
	CanMsgOutgoing.Data[6] = os_max_count & 0xff;
	CanMsgOutgoing.Data[7] = (os_max_count>>8) & 0xff;

	if (CAN1->TSR & CAN_TSR_TME0 || CAN1->TSR & CAN_TSR_TME1 || CAN1->TSR & CAN_TSR_TME2)
	{
		CAN_Transmit(CAN1, &CanMsgOutgoing);
	}

}
